#pragma once
#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>
#include <tuple>

namespace HybridAStar {

struct VehicleParams {
    double length = 4.8;       // 车体总长 (m)
    double width = 2.0;        // 车体宽度 (m)
    double wheelbase = 2.8;    // 轴距 (m)
    double front_overhang = 0.9; // 前悬 (m)
    double rear_overhang = 1.1;  // 后悬 (m)
    double max_steer = M_PI/4; // 最大转向角 (rad)
};

struct State {
    double x, y, theta;       // 后轴中心坐标和航向
    double steer;             // 当前转向角
    int gear = 1;             // 1: 前进, -1: 后退
    double g = 0, h = 0;      // 实际代价和启发代价
    State* parent = nullptr;

    State(double x, double y, double theta, 
         double steer = 0, int gear = 1, 
         State* parent = nullptr)
        : x(x), y(y), theta(theta), 
          steer(steer), gear(gear), parent(parent) {}

    bool operator==(const State& o) const {
        return x == o.x && y == o.y && theta == o.theta;
    }
    //default state constructor
    State() : x(0), y(0), theta(0), steer(0), gear(1), parent(nullptr) {}
};

class OccupancyGrid {
public:
    struct MetaInfo {
        uint32_t width, height;
        double resolution;
        double origin_x, origin_y;
    };

    MetaInfo info;
    std::vector<int8_t> data;

    void load(const MetaInfo& meta, const std::vector<int8_t>& grid) {
        info = meta;
        data = grid;
    }

    bool isOccupied(double wx, double wy) const {
        const int mx = static_cast<int>((wx - info.origin_x) / info.resolution);
        const int my = static_cast<int>((wy - info.origin_y) / info.resolution);
        if (mx < 0 || mx >= info.width || my < 0 || my >= info.height) return true;
        return data[my * info.width + mx] > 50;
    }
};

class HybridAStar {
    struct NodeCompare {
        bool operator()(const State* a, const State* b) const {
            return (a->g + a->h) > (b->g + b->h);
        }
    };

    struct GridKey {
        int x, y, theta;
        bool operator==(const GridKey& o) const {
            return x == o.x && y == o.y && theta == o.theta;
        }
    };

    struct GridHash {
        size_t operator()(const GridKey& k) const {
            return ((k.x * 181243) ^ (k.y * 7177)) + k.theta;
        }
    };

    VehicleParams params_;
    double map_res_ = 0.05;    // 地图分辨率 (m/cell)
    double theta_res_ = 5.0 * M_PI/180; // 航向分辨率
    double step_ = 0.3;        // 运动步长 (m)

public:
    explicit HybridAStar(const VehicleParams& params = VehicleParams(),
                        double resolution = 0.05)
        : params_(params), map_res_(resolution) {
        theta_res_ = (5.0 * M_PI/180) * (0.05/resolution);
    }

    void setMapResolution(double resolution) {
        map_res_ = resolution;
        theta_res_ = (5.0 * M_PI/180) * (0.05/resolution);
    }

    std::vector<State> plan(const State& start, const State& goal, 
                           const OccupancyGrid& map);

private:
    std::vector<State> generateSuccessors(const State& current, 
                                        const OccupancyGrid& map);
    bool checkCollision(const State& s, const OccupancyGrid& map) const;
    GridKey discretize(const State& s) const;
    double heuristic(const State& s, const State& goal) const;
    bool isGoalReached(const State& s, const State& goal) const;
    std::vector<State> tracePath(State* node) const;

    std::array<std::pair<double, double>, 4> 
    getVehicleCorners(const State& s) const;
    bool checkRectCollision(const State& s, const OccupancyGrid& map) const;
};
} // namespace HybridAStar